#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include "MessageFrame.h"
#include <string.h>
#include <arpa/inet.h>
#define BUFF_LEN 1024
#define SERVER_PORT 3000
#define SERVER_IP "192.168.224.101"
#include "v2x_msgs/BSM.h"
#include "v2x_msgs/BSM_multi.h"
#include "v2x_msgs/RSM.h"
#include "v2x_msgs/ParticipantData.h"
#include "v2x_msgs/RSI.h"
#include "v2x_msgs/RTEData.h"
#include "v2x_msgs/RTSData.h"
#include "v2x_msgs/SPAT.h"
#include "v2x_msgs/IntersectionState.h"
#include "v2x_msgs/Phase.h"
#include "v2x_msgs/PhaseState.h"
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <jsoncpp/json/json.h>
#include <fstream>
#include <iostream>

#define MAX_PAYLOAD_LEN 4600

#define CREATE_LONG_TYPE(val, ptr)        \
    (ptr) = (long *)malloc(sizeof(long)); \
    *(ptr) = (val);

// 向ASN SET中添加元素
#define ADD_ASN_SET(LIST, ptr)                            \
                                                          \
    (LIST)->list.array[(((LIST)->list.count)++)] = (ptr); \
    (LIST)->list.size += sizeof(*(ptr));

// 创建ASN SET
#define CREATE_ASN_SET(num, ptr, TYPELIST, TYPE)                 \
    (ptr)->list.count = 0;                                       \
    (ptr)->list.size = 0;                                        \
    (ptr)->list.array = (TYPE **)malloc(sizeof(TYPE *) * (num)); \
    (ptr)->list.free = NULL;

#define CREATE_PositionOffsetLLV(ptr, present_, Lat, Lon)              \
    (ptr)->offsetV = NULL;                                             \
    switch (present_)                                                  \
    {                                                                  \
    case 1:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL1;    \
        (ptr)->offsetLL.choice.position_LL1.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL1.lon = Lon;                 \
        break;                                                         \
    case 2:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL2;    \
        (ptr)->offsetLL.choice.position_LL2.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL2.lon = Lon;                 \
        break;                                                         \
    case 3:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL3;    \
        (ptr)->offsetLL.choice.position_LL3.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL3.lon = Lon;                 \
        break;                                                         \
    case 4:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL4;    \
        (ptr)->offsetLL.choice.position_LL4.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL4.lon = Lon;                 \
        break;                                                         \
    case 5:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL5;    \
        (ptr)->offsetLL.choice.position_LL5.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL5.lon = Lon;                 \
        break;                                                         \
    case 6:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LL6;    \
        (ptr)->offsetLL.choice.position_LL6.lat = Lat;                 \
        (ptr)->offsetLL.choice.position_LL6.lon = Lon;                 \
        break;                                                         \
    case 7:                                                            \
        (ptr)->offsetLL.present = PositionOffsetLL_PR_position_LatLon; \
        (ptr)->offsetLL.choice.position_LatLon.lat = Lat;              \
        (ptr)->offsetLL.choice.position_LatLon.lon = Lon;              \
        break;                                                         \
    default:                                                           \
        break;                                                         \
    }
#define CREATE_BIT_STRING(val, ptr)                    \
    int filed = val / 8;                               \
    int bias = val % 8;                                \
    (ptr)->size = filed + 1;                           \
    (ptr)->buf = (uint8_t *)malloc((ptr)->size);       \
    (ptr)->bits_unused = 0;                            \
    memset((ptr)->buf, 0, sizeof((ptr)->size));        \
    if (val)                                           \
    {                                                  \
        (ptr)->buf[filed] = 0x80;                      \
        (ptr)->buf[filed] = (ptr)->buf[filed] >> bias; \
    }

struct sockaddr_in ser_addr;
int client_fd;

typedef struct len_buf
{
    char *buf;
    int len;
} len_buf_t;

long calcu_timeStamp()
{
    struct timeval time_;
    gettimeofday(&time_, NULL);
    return ((time_.tv_sec % 60 * 1000 + time_.tv_usec / 1000));
}
long calcu_moy()
{
    struct timeval time_;
    gettimeofday(&time_, NULL);
    return ((long)time_.tv_sec / 60) % 525600;
}

//check data
unsigned char check_data(char *data, int length)
{
    char result = 0;
    result = data[0];

    for (int i = 1; i < length; i++)
    {
        result ^= data[i];
    }

    return result;
}

void printf_array(char *buffer, int length)
{
    printf("*************printf array****************\n");
    for (int i = 0; i < length; i++)
    {
        printf("0x%0x ", (unsigned char)buffer[i]);
    }
    printf("\n");
}

len_buf_t *object2asn(MessageFrame_t *messageframe)
{
    len_buf_t *lb = (len_buf_t *)malloc(sizeof(len_buf_t));
    if (messageframe == NULL)
        return NULL;
    char *buf = (char *)malloc(MAX_PAYLOAD_LEN);
    asn_codec_ctx_t *opt_codec_ctx = 0;
    //asn_dec_rval_t rval=uper_encode(opt_codec_ctx,&asn_DEF_MessageFrame,(void**)&messageframe,buf,MAX_PAYLOAD_LEN,0,0);
    asn_enc_rval_t rval = uper_encode_to_buffer(&asn_DEF_MessageFrame, messageframe, buf, MAX_PAYLOAD_LEN);
    if (rval.encoded > 0)
    {
        printf("\n --encode messageFrame OK --\n");
        lb->buf = buf;
        lb->len = (rval.encoded + 7) / 8;
        return lb;
    }
    else
    {
        printf("\n --encode messageFrame Faile --\n");
        return NULL;
    }
}
MessageFrame_t *asn2Object(char *buf, int buflen)
{
    MessageFrame_t *messageframe;
    messageframe = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    asn_codec_ctx_t *opt_codec_ctx = 0;
    asn_dec_rval_t rval = uper_decode(opt_codec_ctx, &asn_DEF_MessageFrame, (void **)&messageframe, buf, buflen, 0, 0);
    if (rval.code == RC_OK)
    {
        printf("\n --decode messageFrame OK --\n");
    }
    else
    {
        messageframe = NULL;
    }
    return messageframe;
}

//bsm_msgs
MessageFrame_t *bsmGen(v2x_msgs::BSM bsm)
{
    MessageFrame_t *mf = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    memset(mf, 0, sizeof(MessageFrame_t));
    mf->present = MessageFrame_PR_bsmFrame;
    BasicSafetyMessage_t *bsminfo = &(mf->choice.bsmFrame);
    bsminfo->msgCnt = 1;
    // bsminfo->id.size = 8;
    // bsminfo->id.buf = (uint8_t *)malloc(8);
    // memcpy(bsminfo->id.buf, rsm.id.data(), 8);
    OCTET_STRING_fromBuf(&(bsminfo->id), bsm.idbuf.data(), bsm.idbuf.size());
    bsminfo->secMark = (long)((ros::Time::now().toNSec() / 1000000) % 60000);
    bsminfo->pos.lat = bsm.lat;
    bsminfo->pos.Long = bsm.lon;
    bsminfo->pos.elevation = 0;
    bsminfo->timeConfidence = NULL;
    bsminfo->posAccuracy = NULL;
    bsminfo->posConfidence = NULL;
    if (bsm.TransmissionState == 0)
    {
        bsminfo->transmission = TransmissionState_neutral;
    }
    else if (bsm.TransmissionState > 0)
    {
        bsminfo->transmission = TransmissionState_forwardGears;
    }
    else if (bsm.TransmissionState < 0)
    {
        bsminfo->transmission = TransmissionState_reverseGears;
    }
    else
    {
        bsminfo->transmission = TransmissionState_unavailable;
    }
    bsminfo->speed = bsm.speed;
    bsminfo->heading = bsm.heading;
    bsminfo->angle = NULL;
    bsminfo->motionCfd = NULL;
    bsminfo->accelSet.lat = bsm.acclat;
    bsminfo->accelSet.Long = bsm.acclon;
    bsminfo->accelSet.vert = bsm.accvert;
    bsminfo->accelSet.yaw = bsm.accyaw;
    struct BrakeSystemStatus brake = {NULL, NULL, NULL, NULL, NULL, NULL, NULL};
    bsminfo->brakes = brake;
    struct VehicleSize vs = {3, 5, NULL};
    bsminfo->size = vs;
    bsminfo->vehicleClass.classification = 1;
    bsminfo->vehicleClass.fuelType = NULL;
    bsminfo->safetyExt = NULL;
    bsminfo->emergencyExt = NULL;
    return mf;
}
//rsm_msgs
MessageFrame_t *rsmGen(v2x_msgs::RSM rsm)
{
    MessageFrame_t *mf = (MessageFrame *)malloc(sizeof(MessageFrame_t));
    memset(mf, 0, sizeof(MessageFrame_t));
    mf->present = MessageFrame_PR_rsmFrame;
    RoadsideSafetyMessage_t *rsminfo = &(mf->choice.rsmFrame);
    // rsminfo->id.size = 8;
    // rsminfo->id.buf = (uint8_t *)malloc(8);
    // memcpy(rsminfo->id.buf, rsm.id.data(), 8);
    OCTET_STRING_fromBuf(&(rsminfo->id), rsm.id.data(), rsm.id.size());
    rsminfo->msgCnt = (rsm.msgcnt++) % 128;
    rsminfo->refPos.lat = rsm.rsu_lat;
    rsminfo->refPos.Long = rsm.rsu_lon;
    rsminfo->refPos.elevation = NULL;
    memset(&(rsminfo->participants.list), 0, sizeof(rsminfo->participants.list));
    CREATE_ASN_SET(rsm.ptlist.size(), &(rsminfo->participants), ParticipantList_t, ParticipantData_t);
    for (int i = 0; i < rsm.ptlist.size(); i++)
    {
        ParticipantData_t *pt = (ParticipantData_t *)malloc(sizeof(ParticipantData_t));
        memset(pt, 0, sizeof(ParticipantData_t));
        pt->ptcId = rsm.ptlist[i].ptcId;
        pt->ptcType = rsm.ptlist[i].SourceType;
        pt->id = NULL;
        pt->secMark = (long)((ros::Time::now().toNSec() / 1000000) % 60000);
        pt->pos.offsetLL.present = PositionOffsetLL_PR_position_LatLon;
        pt->pos.offsetLL.choice.position_LL1.lon = rsm.ptlist[i].lon;
        pt->pos.offsetLL.choice.position_LL1.lat = rsm.ptlist[i].lat;
        pt->pos.offsetV = NULL;
        pt->posConfidence.pos = 12;
        pt->posConfidence.elevation = NULL;
        pt->transmission = NULL;
        pt->speed = rsm.ptlist[i].speed;
        pt->heading = rsm.ptlist[i].heading;
        pt->angle = NULL;
        pt->motionCfd = NULL;
        pt->accelSet = NULL;
        pt->size.length = rsm.ptlist[i].length;
        pt->size.width = rsm.ptlist[i].width;
        if (abs((rsm.ptlist[i].height / 0.05)))
        {
            pt->size.height = (long *)malloc(sizeof(long));
            *(pt->size.height) = (long)abs((rsm.ptlist[i].height / 0.05));
        }
        else
        {
            pt->size.height = NULL;
        }

        pt->vehicleClass = NULL;
        ADD_ASN_SET(&(rsminfo->participants), pt);
    }
    return mf;
}
//rsi_msgs
MessageFrame_t *rsiGen(v2x_msgs::RSI rsi)
{
    MessageFrame_t *mf = (MessageFrame *)malloc(sizeof(MessageFrame_t));
    memset(mf, 0, sizeof(MessageFrame_t));
    mf->present = MessageFrame_PR_rsmFrame;
    RoadSideInformation *rsiinfo = &(mf->choice.rsiFrame);
    // rsiinfo->id.size = 8;
    // rsiinfo->id.buf = (uint8_t *)malloc(8);
    // memcpy(rsiinfo->id.buf, rsi.id.data(), 8);
    OCTET_STRING_fromBuf(&(rsiinfo->id), rsi.id.data(), rsi.id.size());
    rsiinfo->msgCnt = (rsi.msgcnt++) % 128;
    //rsu lat and lon
    rsiinfo->refPos.lat = rsi.rsulat;
    rsiinfo->refPos.Long = rsi.rsulon;
    rsiinfo->refPos.elevation = NULL;
    rsiinfo->rtes = (RTEList_t *)malloc(sizeof(RTEList_t));
    memset(rsiinfo->rtes, 0, sizeof(RTEList_t));
    CREATE_ASN_SET(rsi.rte_list.size(), rsiinfo->rtes, RTEList_t, RTEData_t);
    for (int i = 0; i < rsi.rte_list.size(); i++)
    {
        RTEData_t *rte_temp = (RTEData_t *)malloc(sizeof(RTEData_t));
        memset(rte_temp, 0, sizeof(RTEData_t));
        rte_temp->rteId = i;
        rte_temp->eventType = rsi.rte_list[i].eventType;
        rte_temp->eventSource = rsi.rte_list[i].eventSource;
        rte_temp->eventPos = NULL;
        rte_temp->eventPos = (PositionOffsetLLV_t *)malloc(sizeof(PositionOffsetLLV_t));
        //present = 7
        CREATE_PositionOffsetLLV(rte_temp->eventPos, rsi.rte_list[i].present,
                                 rsi.rte_list[i].lat,
                                 rsi.rte_list[i].lon);
        rte_temp->description = NULL;
        rte_temp->eventRadius = NULL;
        rte_temp->timeDetails = NULL;
        rte_temp->priority = NULL;
        rte_temp->referencePaths = NULL;
        rte_temp->referenceLinks = NULL;
        rte_temp->eventConfidence = NULL;
        ADD_ASN_SET(rsiinfo->rtes, rte_temp);
    }
    rsiinfo->rtss = (RTSList_t *)malloc(sizeof(RTSList_t));
    memset(rsiinfo->rtss, 0, sizeof(RTSList_t));
    CREATE_ASN_SET(rsi.rts_list.size(), rsiinfo->rtss, RTSList_t, RTSData_t);
    for (int i = 0; i < rsi.rts_list.size(); i++)
    {
        RTSData_t *rtss_temp = (RTSData_t *)malloc(sizeof(RTSData_t));
        memset(rsiinfo->rtes, 0, sizeof(RTSData_t));
        rtss_temp->rtsId = i;
        rtss_temp->signType = rsi.rts_list[i].signtype;
        rtss_temp->signPos = NULL;
        rtss_temp->signPos = (PositionOffsetLLV *)malloc(sizeof(PositionOffsetLLV));
        memset(rtss_temp->signPos, 0, sizeof(PositionOffsetLLV));
        CREATE_PositionOffsetLLV(rtss_temp->signPos, rsi.rts_list[i].present,
                                 rsi.rts_list[i].lat,
                                 rsi.rts_list[i].lon);
        rtss_temp->description = NULL;
        rtss_temp->timeDetails = NULL;
        rtss_temp->priority = NULL;
        rtss_temp->referenceLinks = NULL;
        rtss_temp->referencePaths = NULL;
        ADD_ASN_SET(rsiinfo->rtss, rtss_temp);
    }
    return mf;
}
//map_msgs
void mapGen(const Json::Value &root, MapData_t *map_msg)
{

    memset(map_msg, 0, sizeof(MapData_t));

    map_msg->msgCnt = 0;
    // passed minutes of the years

    Json::Value node_root = root["nodes"];
    // nodes list
    CREATE_ASN_SET(node_root.size(), &(map_msg->nodes), NodeList_t, Node_t);

    for (int i = 0; i < node_root.size(); i++)
    {

        Node_t *node_temp = (Node_t *)malloc(sizeof(Node_t));
        memset(node_temp, 0, sizeof(Node_t));
        node_temp->name = OCTET_STRING_new_fromBuf(&asn_DEF_DescriptiveName, node_root[i]["name"].asCString(), node_root[i]["name"].asString().size());

        node_temp->id.region = NULL;
        CREATE_LONG_TYPE((long)node_root[i]["id"]["region"].asInt(), node_temp->id.region);

        node_temp->id.id = (long)node_root[i]["id"]["id"].asInt();

        node_temp->refPos.lat = (long)(node_root[i]["refPos"]["lat"].asDouble() * 1E7);
        node_temp->refPos.Long = (long)(node_root[i]["refPos"]["lon"].asDouble() * 1E7);
        node_temp->refPos.elevation = NULL;

        Json::Value root_inLinks = node_root[i]["inLinks"];
        if (root_inLinks.size() == 0)
        {
            node_temp->inLinks = NULL;
            ADD_ASN_SET(&(map_msg->nodes), node_temp);
            continue;
        }

        node_temp->inLinks = (LinkList_t *)malloc(sizeof(LinkList_t));
        memset(node_temp->inLinks, 0, sizeof(LinkList_t));
        CREATE_ASN_SET(root_inLinks.size(), node_temp->inLinks, LinkList_t, Link_t);

        for (int j = 0; j < root_inLinks.size(); ++j)
        {

            Link_t *link_temp = (Link_t *)malloc(sizeof(Link_t));
            memset(link_temp, 0, sizeof(Link_t));
            link_temp->name = OCTET_STRING_new_fromBuf(&asn_DEF_DescriptiveName, root_inLinks[j]["name"].asCString(), root_inLinks[j]["name"].asString().size());

            link_temp->upstreamNodeId.region = NULL;
            CREATE_LONG_TYPE((long)root_inLinks[j]["upstreamNodeId"]["region"].asInt(), link_temp->upstreamNodeId.region);

            link_temp->upstreamNodeId.id = (long)root_inLinks[j]["upstreamNodeId"]["id"].asInt();
            // TODO: 增加speed消息

            Json::Value speedLimits_root = root_inLinks[j]["speedLimits"];
            if (speedLimits_root.empty())
            {
                link_temp->speedLimits = NULL;
            }
            else
            {

                link_temp->speedLimits = (SpeedLimitList_t *)malloc(sizeof(SpeedLimitList_t));
                memset(link_temp->speedLimits, 0, sizeof(SpeedLimitList_t));
                CREATE_ASN_SET(speedLimits_root.size(), link_temp->speedLimits, SpeedLimitList_t, RegulatorySpeedLimit_t)

                for (int u = 0; u < speedLimits_root.size(); ++u)
                {
                    RegulatorySpeedLimit_t *speedlimit_temp = (RegulatorySpeedLimit_t *)malloc(sizeof(RegulatorySpeedLimit_t));
                    memset(speedlimit_temp, 0, sizeof(RegulatorySpeedLimit_t));

                    speedlimit_temp->type = speedLimits_root[u]["type"].asInt();
                    speedlimit_temp->speed = (long)(speedLimits_root[u]["speed"].asInt() / 0.02);

                    ADD_ASN_SET(link_temp->speedLimits, speedlimit_temp);
                }
            }

            // linkWidth的分辨率为cm
            CREATE_LONG_TYPE((long)root_inLinks[j]["laneWidth"].asInt() * 100, link_temp->linkWidth);
            Json::Value link_points_root = root_inLinks[j]["points"];
            if (link_points_root.empty())
            {
                link_temp->points = NULL;
            }
            else
            {
                // 添加points
                link_temp->points = (PointList_t *)malloc(sizeof(PointList_t));
                memset(link_temp->points, 0, sizeof(PointList_t));
                CREATE_ASN_SET(link_points_root.size(), link_temp->points, PointList_t, RoadPoint_t);

                for (int u = 0; u < link_points_root.size(); ++u)
                {

                    RoadPoint_t *roadpoint_temp = (RoadPoint_t *)malloc(sizeof(RoadPoint_t));
                    memset(roadpoint_temp, 0, sizeof(RoadPoint_t));

                    CREATE_PositionOffsetLLV(&(roadpoint_temp->posOffset), link_points_root[u]["offsetLL"]["present"].asInt(),
                                             (long)(link_points_root[u]["offsetLL"]["choice"]["lat"].asDouble() * 1E7),
                                             (long)(link_points_root[u]["offsetLL"]["choice"]["lon"].asDouble() * 1E7));

                    roadpoint_temp->posOffset.offsetV = NULL;

                    ADD_ASN_SET(link_temp->points, roadpoint_temp);
                }
            }

            Json::Value movements_root = root_inLinks[j]["movements"];
            // TODO: 添加movement
            if (movements_root.empty())
            {
                link_temp->movements = NULL;
            }
            else
            {

                link_temp->movements = (MovementList_t *)malloc(sizeof(MovementList_t));
                memset(link_temp->movements, 0, sizeof(MovementList_t));
                CREATE_ASN_SET(movements_root.size(), link_temp->movements, MovementList_t, Movement_t);

                for (int u = 0; u < movements_root.size(); ++u)
                {
                    Movement_t *movement_temp = (Movement_t *)malloc(sizeof(Movement_t));
                    memset(movement_temp, 0, sizeof(Movement_t));

                    CREATE_LONG_TYPE((long)movements_root[u]["remoteIntersection"]["region"].asInt(),
                                     movement_temp->remoteIntersection.region);
                    movement_temp->remoteIntersection.id = movements_root[u]["remoteIntersection"]["id"].asInt();

                    CREATE_LONG_TYPE((long)movements_root[u]["phaseId"].asInt(), movement_temp->phaseId);

                    ADD_ASN_SET(link_temp->movements, movement_temp);
                }
            }

            Json::Value lanes_root = root_inLinks[j]["lanes"];
            // TODO: 添加lanes
            CREATE_ASN_SET(lanes_root.size(), &(link_temp->lanes), LaneList_t, Lane_t);

            for (int u = 0; u < lanes_root.size(); ++u)
            {

                Lane_t *lane_temp = (Lane_t *)malloc(sizeof(Lane_t));
                memset(lane_temp, 0, sizeof(Lane_t));
                lane_temp->laneID = (long)lanes_root[u]["laneID"].asInt();

                CREATE_LONG_TYPE((long)lanes_root[u]["laneWidth"].asInt(), lane_temp->laneWidth);

                lane_temp->laneAttributes = NULL;
                lane_temp->maneuvers = NULL;
                lane_temp->connectsTo = NULL;
                if (root_inLinks[j]["lanes"][u]["speedLimits"].empty())
                {
                    lane_temp->speedLimits = NULL;
                }
                else
                {
                    lane_temp->speedLimits = (SpeedLimitList_t *)malloc(sizeof(SpeedLimitList_t));
                    memset(lane_temp->speedLimits, 0, sizeof(SpeedLimitList_t));
                    CREATE_ASN_SET(root_inLinks[j]["lanes"][u]["speedLimits"].size(), lane_temp->speedLimits, SpeedLimitList_t, RegulatorySpeedLimit_t);

                    for (int v = 0; v < root_inLinks[j]["lanes"][u]["speedLimits"].size(); v++)
                    {
                        RegulatorySpeedLimit_t *speedlimit_temp = (RegulatorySpeedLimit_t *)malloc(sizeof(RegulatorySpeedLimit_t));
                        memset(speedlimit_temp, 0, sizeof(RegulatorySpeedLimit_t));

                        speedlimit_temp->type = root_inLinks[j]["lanes"][u]["speedLimits"][v]["type"].asInt();
                        speedlimit_temp->speed = (long)(root_inLinks[j]["lanes"][u]["speedLimits"][v]["speed"].asInt() / 0.02);

                        ADD_ASN_SET(lane_temp->speedLimits, speedlimit_temp);
                    }
                }
                if (root_inLinks[j]["lanes"][u]["points"].empty())
                {
                    lane_temp->points = NULL;
                }
                else
                {
                    lane_temp->points = (PointList_t *)malloc(sizeof(PointList_t));
                    memset(lane_temp->points, 0, sizeof(PointList_t));
                    CREATE_ASN_SET(root_inLinks[j]["lanes"][u]["points"].size(), lane_temp->points, PointList_t, RoadPoint_t);
                    Json::Value points_root = root_inLinks[j]["lanes"][u]["points"];

                    for (int v = 0; v < points_root.size(); ++v)
                    {
                        RoadPoint_t *point_temp = (RoadPoint_t *)malloc(sizeof(RoadPoint_t));
                        CREATE_PositionOffsetLLV(&(point_temp->posOffset), points_root[v]["offsetLL"]["present"].asInt(),
                                                 (long)(points_root[v]["offsetLL"]["choice"]["lat"].asDouble() * 1E7),
                                                 (long)(points_root[v]["offsetLL"]["choice"]["lon"].asDouble() * 1E7));

                        ADD_ASN_SET(lane_temp->points, point_temp);
                    }
                }

                ADD_ASN_SET(&(link_temp->lanes), lane_temp);
            }
            ADD_ASN_SET(node_temp->inLinks, link_temp);
        }

        ADD_ASN_SET(&(map_msg->nodes), node_temp);
    }
}
//spat_msgs
MessageFrame_t *spatGen(v2x_msgs::SPAT spat)
{
    MessageFrame_t *mf = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    memset(mf, 0, sizeof(MessageFrame_t));
    mf->present = MessageFrame_PR_spatFrame;
    SPAT_t *spatinfo = &(mf->choice.spatFrame);
    memset(spatinfo, 0, sizeof(SPAT_t));
    spatinfo->msgCnt = 1;
    CREATE_LONG_TYPE(calcu_moy(), spatinfo->moy);
    if (spat.intersections.empty())
    {
        return mf;
    }

    CREATE_ASN_SET(spat.intersections.size(), &(spatinfo->intersections), IntersectionStateList_t, IntersectionState_t);
    for (int i = 0; i < spat.intersections.size(); i++)
    {
        IntersectionState_t *is = (IntersectionState_t *)malloc(sizeof(IntersectionState_t));
        memset(is, 0, sizeof(IntersectionState_t));
        CREATE_LONG_TYPE(1, is->intersectionId.region);
        is->intersectionId.id = spat.intersections[i].nodeID;
        // is->status = IntersectionStatusObject_fixedTimeOperation; BIT_STRING
        CREATE_BIT_STRING(5, &(is->status));
        CREATE_LONG_TYPE((long)((ros::Time::now().toNSec() / 1000000) % 60000), is->timeStamp);

        memset(&(is->phases), 0, sizeof(PhaseList_t));

        CREATE_ASN_SET(spat.intersections[i].phases.size(), &(is->phases), PhaseList_t, Phase_t);
        for (int j = 0; j < spat.intersections[i].phases.size(); j++)
        {
            Phase_t *pl = (Phase_t *)malloc(sizeof(Phase_t));
            memset(pl, 0, sizeof(Phase_t));
            pl->id = spat.intersections[i].phases[j].id;
            memset(&(pl->phaseStates), 0, sizeof(PhaseStateList_t));
            CREATE_ASN_SET(spat.intersections[i].phases[j].phaseStates.size(), &(pl->phaseStates), PhaseStateList_t, PhaseState_t);
            for (int u = 0; u < spat.intersections[i].phases[j].phaseStates.size(); u++)
            {
                PhaseState_t *ps = (PhaseState_t *)malloc(sizeof(PhaseState_t));
                memset(ps, 0, sizeof(PhaseState_t));
                ps->light = spat.intersections[i].phases[j].phaseStates[u].LightState;
                ps->timing = (TimeChangeDetails_t *)malloc(sizeof(TimeChangeDetails_t));
                memset(ps->timing, 0, sizeof(TimeChangeDetails_t));
                ps->timing->present = TimeChangeDetails_PR_counting;
                TimeCountingDown *tc = &(ps->timing->choice.counting);
                tc->startTime = spat.intersections[i].phases[j].phaseStates[u].startTime;
                tc->likelyEndTime = spat.intersections[i].phases[j].phaseStates[u].likelyEndTIme;
                tc->maxEndTime = NULL;
                tc->minEndTime = NULL;
                tc->nextDuration = NULL;
                tc->nextStartTime = NULL;
                tc->timeConfidence = NULL;
                ADD_ASN_SET(&(pl->phaseStates), ps);
            }
            ADD_ASN_SET(&(is->phases), pl);
        }
        ADD_ASN_SET(&(spatinfo->intersections), is);
    }
    return mf;
}
//send messages to obu
void sendV2X(int client_fd, struct sockaddr_in server, len_buf_t *result)
{
    ssize_t readlength = 0;
    int len = result->len;
    char *buf = result->buf;
    //buffer==v2x
    char buffer[BUFF_LEN];
    buffer[0] = 0xAA;
    buffer[1] = 0x75;
    buffer[2] = 0x02;
    buffer[3] = 0x00;
    buffer[4] = len + 5;
    buffer[5] = 0x04;
    buffer[6] = 0x00;
    buffer[7] = 0x3d;
    buffer[8] = 0x00;
    buffer[9] = len;
    memcpy(buffer + 10, buf, len);
    printf_array(result->buf, result->len);
    buffer[10 + len] = check_data(buffer, 10 + len);
    printf("send data\n");
    printf_array(buffer, 11 + len);
    sendto(client_fd, buffer, 11 + len, 0, (struct sockaddr *)&ser_addr, sizeof(ser_addr));
    printf("sent\n");
    char buffer_back[7];
    readlength = recvfrom(client_fd, buffer_back, sizeof(buffer_back), 0, NULL, NULL);
    printf("server ack# \r\n");
    printf_array(buffer_back, readlength);
    printf("server ack end# \r\n");
}
//bsm_callback
void bsm_call_back(const v2x_msgs::BSM_multi &msg)
{

    uint8_t len = msg.size;

    for (int i = 0; i < len; i++)
    {
        v2x_msgs::BSM bsm = msg.data[i];
        //send bsm
        MessageFrame_t *mf_bsm = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
        mf_bsm = bsmGen(bsm);
        len_buf_t *result = object2asn(mf_bsm);
        // MessageFrame_t *rval = asn2Object(result->buf, MAX_PAYLOAD_LEN);
        // printf("-- end decode --\n");
        // xer_fprint(stdout, &asn_DEF_MessageFrame, rval);
        sendV2X(client_fd, ser_addr, result);
        printf("BSM SEND!");
        ASN_STRUCT_FREE(asn_DEF_MessageFrame, mf_bsm);
    }
}
//rsm_callback
void rsm_call_back(const v2x_msgs::RSM &msg)
{
    MessageFrame_t *mf_rsm = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    mf_rsm = rsmGen(msg);
    len_buf_t *result = object2asn(mf_rsm);
    //MessageFrame_t *rval = asn2Object(result->buf, MAX_PAYLOAD_LEN);
    //printf("-- end encode --\n");
    //xer_fprint(stdout, &asn_DEF_MessageFrame, rval);
    xer_fprint(stdout, &asn_DEF_MessageFrame, mf_rsm);
    sendV2X(client_fd, ser_addr, result);
    printf("RSM SEND!");
    ASN_STRUCT_FREE(asn_DEF_MessageFrame, mf_rsm);
}
//rsi_callback
void rsi_call_back(const v2x_msgs::RSI &msg)
{
    MessageFrame_t *mf_rsi = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    printf("2");
    mf_rsi = rsiGen(msg);
    // len_buf_t *result = object2asn(mf_rsi);
    // //MessageFrame_t *rval = asn2Object(result->buf, MAX_PAYLOAD_LEN);
    // //printf("-- end encode --\n");
    // xer_fprint(stdout, &asn_DEF_MessageFrame, mf_rsi);
    // sendV2X(client_fd, ser_addr, result);
    // printf("RSI SEND!");
    // ASN_STRUCT_FREE(asn_DEF_MessageFrame, mf_rsi);
    
}
//read file map.json and mapGen
void read_map_message_from_file(std::string filename, MapData_t *map_msg)
{

    // open json file
    std::ifstream ifs;
    ifs.open(filename, std::ios::binary);

    if (!ifs.is_open())
    {
        printf("FILE OPEN FILED!");
    }

    Json::Reader reader;
    Json::Value root;

    if (reader.parse(ifs, root, true) == -1)
    {
        ifs.close();
    }

    //  MapData_t* map_msg = &(messageframe->choice.mapFrame);
    mapGen(root, map_msg);
    ifs.close();
}
//send map message with timestamp
void send_map_message(std::string filename)
{

    MessageFrame_t *mf_map = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    memset(mf_map, 0, sizeof(MessageFrame_t));
    mf_map->present = MessageFrame_PR_mapFrame;
    read_map_message_from_file(filename, &(mf_map->choice.mapFrame));
    long timestamp = calcu_moy();
    CREATE_LONG_TYPE(timestamp, mf_map->choice.mapFrame.timeStamp);
    //xer_fprint(stdout, &asn_DEF_MessageFrame, mf_map);
    len_buf_t *result = object2asn(mf_map);
    sendV2X(client_fd, ser_addr, result);
    printf("MAP SEND!");
    ASN_STRUCT_FREE(asn_DEF_MessageFrame, mf_map);
}
//spat_callback
void spat_call_back(const v2x_msgs::SPAT &msg)
{
    MessageFrame_t *mf_spat = (MessageFrame_t *)malloc(sizeof(MessageFrame_t));
    mf_spat = spatGen(msg);
    len_buf_t *result = object2asn(mf_spat);
    //MessageFrame_t *rval = asn2Object(result->buf, MAX_PAYLOAD_LEN);
    //printf("-- end encode --\n");
    //xer_fprint(stdout, &asn_DEF_MessageFrame, rval);
    sendV2X(client_fd, ser_addr, result);
    printf("SPAT SEND!");
    ASN_STRUCT_FREE(asn_DEF_MessageFrame, mf_spat);
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "bsm_send");
    ros::NodeHandle nh;
    // client_fd = socket(AF_INET, SOCK_DGRAM, 0); //UDP
    // if (client_fd < 0)
    // {
    //     perror("socket");
    // }
    ROS_INFO("init node");
    // ser_addr.sin_family = AF_INET;
    // ser_addr.sin_addr.s_addr = inet_addr(SERVER_IP);
    // ser_addr.sin_port = htons(SERVER_PORT);
    //v2x_msgs::BSM bsm;
    //xer_fprint(stdout, &asn_DEF_MessageFrame, mf);
    //ros::Subscriber sb = nh.subscribe("/bsm", 1, bsm_call_back);
    //ros::Subscriber sr = nh.subscribe("/rsm", 1, rsm_call_back);
    ros::Subscriber sri = nh.subscribe("/rsi", 1, rsi_call_back);
    // send_map_message("/home/dingxin/zhengyuan_obu/src/bsm_send/src/map.json");
    // ros::Subscriber sspat = nh.subscribe("/spat", 1, spat_call_back);
    ros::spin();
   // close(client_fd);
}
